#include "rads_tbox.hpp"

MultListen mult_listen;
// 全局原子变量，用于线程间的通信
std::atomic<bool> should_stop(false);
std::condition_variable cv;
std::mutex mtx;
ros::Publisher p;

void threadtbox_core(ros::NodeHandle& nh,MultListen& mult_listen)
{
    int heartbeat = 0;
    PrevCmdRecord prevCmd;
    mult_listen._vri = {KTV_SN, 0, -1,119,32,0,99.99,100,1,1,0,-1,-1,-1,0,0,0,0,-1,-1,1};
 	std::string cloud_message;
    while(!should_stop.load())
    {
	    TCPClient client(CLOUD_SERVER, CLOUD_PORT);
        while(!client.connectToServer()&&!should_stop.load())
        {
            ROS_WARN("server connect fail.Check whether the remote server is enabled");
            std::this_thread::sleep_for(std::chrono::seconds(1));
        }
	    while(!client.authenticate(CLOUD_USERNAME, CLOUD_PASSWORD)&&!should_stop.load())
        {
	        ROS_WARN("Waiting for system authorization, please wait");
            std::this_thread::sleep_for(std::chrono::seconds(1));
        }
	    while(!should_stop.load())
	    {
            sleep(1);
            // 下位机
	        mult_listen._vri.ts = current_timestamp_ms();
            mult_listen._vri.model = mult_listen._scmd.model;
            mult_listen._vri.soc = mult_listen._scmd.soc;
            // 定位模块
            mult_listen._vri.lng = mult_listen._gnss.longitude;
            mult_listen._vri.lat = mult_listen._gnss.latitude;
            mult_listen._vri.alt = mult_listen._gnss.altitude;
            mult_listen._vri.fix_posi = mult_listen._gnss.posi_flag;
            mult_listen._vri.fix_head = mult_listen._gnss.head_flag;
            mult_listen._vri.heading = mult_listen._gnss.heading;

            // 融合数据
	        mult_listen._vri.fx = mult_listen._fusion.x;
	        mult_listen._vri.fy = mult_listen._fusion.y;
	        mult_listen._vri.fz = mult_listen._fusion.z;
	        mult_listen._vri.yaw = mult_listen._fusion.yaw;
	        mult_listen._vri.ndt = mult_listen._fusion.matching_rate;
	        mult_listen._vri.speed = mult_listen._fusion.groud_speed;

            // 心跳[0-255]
	        heartbeat  = ++heartbeat > 256? heartbeat : 0;
	        mult_listen._vri.heartbeat = heartbeat;
	        json demo = mult_listen._vri;
	        std::string demostr = demo.dump();
            if(0 == client.GetErr()){
                mult_listen.tbox_report = 1;
		        client.sendData(demostr);
                mult_listen.tbox_report = -1;
		        client.receiveDataNoblock(cloud_message);
                if(!cloud_message.empty())
                {
                    if(isValidNetCommand(cloud_message))
                    {
                        ROS_INFO("receive network instructions [%s].", cloud_message.c_str());
                        NetCmdObsolete cmd = DecodeNetCmdObsolete(cloud_message);
                        Cmd2radsmsg(cmd,mult_listen._cmd);
                        if(prevCmd.prev_nema != mult_listen._cmd.pro_nema)
                        {
                            prevCmd.prev_nema = mult_listen._cmd.pro_nema;
                            prevCmd.prev_nema_ts = current_timestamp_ms();
                            p.publish(mult_listen._cmd);
                            ROS_WARN(">>>[ New ] instructions are being issued to the system");
                        }else{
                            if(current_timestamp_ms() > (prevCmd.prev_nema_ts + 5*1000))
                            {
                                prevCmd.prev_nema_ts = current_timestamp_ms();
                                p.publish(mult_listen._cmd);
                                ROS_WARN(">>> [ Same ] instructions2 are being issued to the system");
                            }
                        }
                    }else{
                        ROS_ERROR("Please issue legal control commands,err code = %s",cloud_message.c_str());
                    }
                    cloud_message = "";
                }
            }else{
                ROS_WARN("The remote service may have been abnormally shut down = %i",client.GetErr());
                client.selfClose();
                std::this_thread::sleep_for(std::chrono::milliseconds(100));
                break;
            }
	        std::unique_lock<std::mutex> lck(mtx);
	        cv.wait_for(lck, std::chrono::milliseconds(1000), [] { return should_stop.load(); });
	    }
    }
}

void log_core(MultListen& mult_listen)
{
    CacheLog log;
    bool lock = false;
    while(!should_stop.load())
    {
        lock = false;
        if(-1 == mult_listen.tbox_report)
        {
            if(!lock && mult_listen.logs.fatal.empty())
            {
                lock = true;
                log = mult_listen.logs.fatal.front();
                mult_listen.logs.fatal.pop();
                mult_listen._vri.err = log.code;
            }
            if(!lock && mult_listen.logs.error.empty())
            {
                lock = true;
                log = mult_listen.logs.error.front();
                mult_listen.logs.error.pop();
                mult_listen._vri.err = log.code;
            }
            if(!lock && mult_listen.logs.warn.empty())
            {
                lock = true;
                log = mult_listen.logs.warn.front();
                mult_listen.logs.warn.pop();
                mult_listen._vri.err = log.code;
            }
            if(!lock && mult_listen.logs.info.empty())
            {
                lock = true;
                log = mult_listen.logs.info.front();
                mult_listen.logs.info.pop();
                mult_listen._vri.err = log.code;
            }
            if(!lock && mult_listen.logs.debug.empty())
            {
                lock = true;
                log = mult_listen.logs.debug.front();
                mult_listen.logs.debug.pop();
                mult_listen._vri.err = log.code;
            }
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(30));
    }
}


// 订阅来自下位机的状态数据
void cmd_callback(const rads::rads_scmd& _cmd)
{
    mult_listen._scmd.model = _cmd.model;
    mult_listen._scmd.soc = _cmd.soc;
    mult_listen._scmd.err = _cmd.err;
    mult_listen._scmd.rpm_l = _cmd.rpm_l;
    mult_listen._scmd.rpm_r = _cmd.rpm_r;
    mult_listen._scmd.key_1 = _cmd.key_1;
    mult_listen._scmd.key_2 = _cmd.key_2;
    mult_listen._scmd.key_3 = _cmd.key_3;
    mult_listen._scmd.key_4 = _cmd.key_4;
    mult_listen._scmd.key_5 = _cmd.key_5;
    mult_listen._scmd.key_6 = _cmd.key_6;
    mult_listen._scmd.key_7 = _cmd.key_7;
    mult_listen._scmd.key_8 = _cmd.key_8;
    mult_listen._scmd.unknow_1 = _cmd.unknow_1;
    mult_listen._scmd.unknow_2 = _cmd.unknow_2;
    mult_listen._scmd.unknow_3 = _cmd.unknow_3;
    mult_listen._scmd.unknow_4 = _cmd.unknow_4;
    mult_listen._scmd.unknow_5 = _cmd.unknow_5;
    mult_listen._scmd.unknow_6 = _cmd.unknow_6;
}

// 订阅系统监控信息
void guard_callback(const rads::rads_guard& _guard)
{
    mult_listen._guard.system_version = _guard.system_version;
    mult_listen._guard.kernel_version = _guard.kernel_version;
    mult_listen._guard.robot_version = _guard.robot_version;
    mult_listen._guard.cpu_usage = _guard.cpu_usage;
    mult_listen._guard.memery_usage = _guard.memery_usage;
    mult_listen._guard.api_alive = _guard.api_alive;
    mult_listen._guard.business_alive = _guard.business_alive;
    mult_listen._guard.database_alive = _guard.database_alive;
    mult_listen._guard.drive_alive = _guard.drive_alive;
    mult_listen._guard.rtk_alive = _guard.rtk_alive;
    mult_listen._guard.slam_alive = _guard.slam_alive;
    mult_listen._guard.tbox_alive = _guard.tbox_alive;
    mult_listen._guard.screen_alive = _guard.screen_alive;
    mult_listen._guard.map_alive = _guard.map_alive;
    mult_listen._guard.rads_ready = _guard.rads_ready;
    mult_listen._guard.err = _guard.err;
    mult_listen._guard.ext1 = _guard.ext1;
    mult_listen._guard.ext2 = _guard.ext2;
    mult_listen._guard.ext3 = _guard.ext3;
    mult_listen._guard.ext4 = _guard.ext4;
    mult_listen._guard.ext5 = _guard.ext5;
    mult_listen._guard.ext6 = _guard.ext6;
}

// 订阅系统定位信息
void rtk_callback(const rads::rads_trackext& _rtk)
{
    mult_listen._gnss.longitude = _rtk.longitude;
    mult_listen._gnss.latitude = _rtk.latitude;
    mult_listen._gnss.altitude = _rtk.altitude;
    mult_listen._gnss.heading = _rtk.heading;
    mult_listen._gnss.posi_flag = _rtk.posi_flag;
    mult_listen._gnss.head_flag = _rtk.head_flag;
}

// 订阅融合定位信息
void fusion_callback(const rads::rads_trackext& _fusion)
{
    mult_listen._fusion.x = _fusion.x;
    mult_listen._fusion.y = _fusion.y;
    mult_listen._fusion.z = _fusion.z;
    mult_listen._fusion.yaw = _fusion.yaw;
    mult_listen._fusion.matching_rate = _fusion.matching_rate;
    mult_listen._fusion.groud_speed = _fusion.groud_speed;
    mult_listen._fusion.ext = _fusion.ext;
    mult_listen._fusion.flag = _fusion.flag;
}

// 订阅来自日志消息
void log_callback(const rads::rads_log& _log)
{
    CacheLog log,last_log;
    CacheMsg2Log(log,_log);
    switch(_log.level)
    {
        case DEBUG_LEVEL_NONE:
            {
                if(!mult_listen.logs.debug.empty())
                {
                    last_log = mult_listen.logs.debug.back();
                    if(log.code != last_log.code)mult_listen.logs.debug.push(log);
                }else{
                    mult_listen.logs.debug.push(log);
                }
            }
            break;
        case DEBUG_LEVEL_INFO:
            {
                if(!mult_listen.logs.info.empty())
                {
                    last_log = mult_listen.logs.info.back();
                    if(log.code != last_log.code)mult_listen.logs.info.push(log);
                }else{
                    mult_listen.logs.info.push(log);
                }
            }
            break;
        case DEBUG_LEVEL_WARN:
            {
              if(!mult_listen.logs.warn.empty())
              {
                  last_log = mult_listen.logs.warn.back();
                  if(log.code != last_log.code)mult_listen.logs.warn.push(log);
              }else{
                  mult_listen.logs.warn.push(log);
              }
            }
            break;
        case DEBUG_LEVEL_ERROR:
            {
                if(!mult_listen.logs.error.empty())
                {
                    last_log = mult_listen.logs.error.back();
                    if(log.code != last_log.code)mult_listen.logs.error.push(log);
                }else{
                    mult_listen.logs.error.push(log);
                }
            }
            break;
        case DEBUG_LEVEL_FATAL:
            {
                if(!mult_listen.logs.fatal.empty())
                {
                    last_log = mult_listen.logs.fatal.back();
                    if(log.code != last_log.code)mult_listen.logs.fatal.push(log);
                }else{
                    mult_listen.logs.fatal.push(log);
                }
            }
            break;
        default:
            break;
    }
}

// 订阅来自算法层的响应确认
void ack_callback(const rads::rads_acks& ack)
{
    switch(ack.cmd)
    {
        case CMDO_MAP_BUILD:
            // 结束建图
            if(CMDO_END == ack.ext2)
            {
                ROS_WARN("End Create Map,mapid = %zu,Start synchronizing map data.",ack.map_id);
                if(CMDO_ACK_BEGIN == ack.ack){
                    mult_listen._api.map_path = ack.map_path;
                    mult_listen._api.map_id = ack.map_id;
                    mult_listen._api.cmd = CMDO_MAP_BUILD;
                    mult_listen._api.flag = 1;
                    mult_listen._api.ext = CMDO_ACK_END;
                }
            }
            // 开始建图
            else if(CMDO_BEGIN == ack.ext2)
            {
                if(CMDO_ACK_BEGIN == ack.ack)
                {
                    ROS_WARN("Start building maps,mapid = %zu",ack.map_id);
                }else if(CMDO_ACK_END == ack.ack){
                    ROS_ERROR("The system cannot provide building mapid [%zu] service for you",ack.map_id);
                }
            }
            break;
        case CMDO_PATH_BUILD:
            // 开始建路径
            if(CMDO_BEGIN == ack.ext2)
            {
                if(CMDO_ACK_BEGIN == ack.ack){
                    ROS_WARN("Start path building,mapid = %zu,path_id = %zu",ack.map_id,ack.path_id);
                    mult_listen._api.map_path = ack.map_path;
                    mult_listen._api.map_id = ack.map_id;
                    mult_listen._api.path_id = ack.path_id;
                    mult_listen._api.cmd = CMDO_PATH_BUILD;
                    mult_listen._api.flag = 1;
                    mult_listen._api.ext = CMDO_ACK_BEGIN;
                }else if (CMDO_ACK_LOADING == ack.ack)
                {
                    ROS_WARN("Start uploading points,mapid = %zu,path_id = %zu,point = %zu",ack.map_id,ack.path_id,ack.trajectory_ext[0].point_count);
                    mult_listen._api.map_path = ack.map_path;
                    mult_listen._api.map_id = ack.map_id;
                    mult_listen._api.path_id = ack.path_id;
                    mult_listen._api.cmd = CMDO_PATH_BUILD;
                    mult_listen._api.flag = 1;
                    mult_listen._api.ext = CMDO_ACK_LOADING;
                    mult_listen._api.trajectory_ext.point_count = ack.trajectory_ext[0].point_count;
                    if(ack.trajectory_ext.size() > 0)
                    {
                        mult_listen._api.trajectory_ext.x = ack.trajectory_ext[0].x;
                        mult_listen._api.trajectory_ext.y = ack.trajectory_ext[0].y;
                        mult_listen._api.trajectory_ext.z = ack.trajectory_ext[0].z;
                        mult_listen._api.trajectory_ext.yaw = ack.trajectory_ext[0].yaw;
                        mult_listen._api.trajectory_ext.matching_rate = ack.trajectory_ext[0].matching_rate;
                        mult_listen._api.trajectory_ext.longitude = ack.trajectory_ext[0].longitude;
                        mult_listen._api.trajectory_ext.latitude = ack.trajectory_ext[0].latitude;
                        mult_listen._api.trajectory_ext.altitude = ack.trajectory_ext[0].altitude;
                        mult_listen._api.trajectory_ext.heading = ack.trajectory_ext[0].heading;
                        mult_listen._api.trajectory_ext.posi_flag = ack.trajectory_ext[0].posi_flag;
                        mult_listen._api.trajectory_ext.head_flag = ack.trajectory_ext[0].head_flag;
                    }
                }
            }
            // 结束闭环路径
            else if(CMDO_END == ack.ext2)
            {
                if(CMDO_ACK_BEGIN == ack.ack)
                {
                    ROS_WARN("Closing loop path,mapid = %zu,path_id =  %zu",ack.map_id,ack.path_id);
                }
            }
        default:break;
    }
}

// 信号处理函数
void signalHandler(int signum) {
    if(SIGINT == signum)
    {
        system("killall -s SIGTERM rads_guard");
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        ROS_INFO("[Tbox] Interrupt signal (%d) received. Shutting down...", signum);
        std::lock_guard<std::mutex> lck(mtx);
        should_stop.store(true);
        cv.notify_all();
        ros::shutdown();
    }
}

int main(int argc, char **argv)
{
    ROS_INFO("[Tbox] Starting....");
	ros::init(argc, argv, NODE_TBOX);
	ros::NodeHandle nh;
    // 设置信号处理函数
    signal(SIGINT, signalHandler);
    ros::param::get("KTV_SN",KTV_SN);
    mult_listen._api.ext = 0;
	std::thread t_tbox(threadtbox_core, std::ref(nh),std::ref(mult_listen));
    std::thread t_log(log_core, std::ref(mult_listen));
    // 1.发布来自网络的简要指令集
    p = nh.advertise<rads::rads_cmd>("/pradscmd", QUEUE_SIZE);
    // 2.订阅下位机的上报数据
    ros::Subscriber p2 = nh.subscribe("/pradsscmd", QUEUE_SIZE,cmd_callback);
    // 3.订阅系统ACK指令
    ros::Subscriber s = nh.subscribe("/sradsack", QUEUE_SIZE,ack_callback);
    // 4.订阅系统监控信息
    ros::Subscriber s2 = nh.subscribe("/pradsguard", QUEUE_SIZE,guard_callback);
    // 5.订阅系统定位信息
    ros::Subscriber s3 = nh.subscribe("/pradsrtk", QUEUE_SIZE,rtk_callback);
    // 6.订阅融合数据
    ros::Subscriber s4 = nh.subscribe("/pradsfusion", QUEUE_SIZE,fusion_callback);
    // 7.订阅日志信息
    ros::Subscriber s5 = nh.subscribe("/sradslog", QUEUE_SIZE,log_callback);
    // 8.部分需要转接到API接口服务
    ros::Publisher papi = nh.advertise<rads::rads_apis>("/pradsapi", QUEUE_SIZE);
	ros::Rate rate(100);
	while (nh.ok() && !should_stop.load())
	{
        if(1 == mult_listen._api.flag)
        {
	        mult_listen._api.header.stamp = ros::Time::now();
	        mult_listen._api.header.frame_id = "nyd";
	        papi.publish(mult_listen._api);
            mult_listen._api.flag = 0;
        }
        ros::spinOnce();
		rate.sleep();
	}
	t_tbox.join();
	ros::spin();
	return 0;
}
